Manipulation planning: building paths on constrained manifolds

Constrained motion planning and Manipulation planning, for generic non-linear constraints, highly rely on the ability of solving non-linear equations. The Newton-Raphson method, often used in this context, is discontinuous with respect to its input and point wise path projection can lead to discontinuous constrained paths. The discontinuities come from the pseudo-inverse involved at each iteration of the NewtonRaphson algorithm. An interval of continuity for one iteration is derived from an upper bound of the norm of the Hessian of the constraints. Such a bound is easy to compute for constraints involving joint positions and orientations. We introduce two heuristical path projection algorithms and empirically show that they generate continuous constrained paths. The algorithms are integrated into a motion planning algorithm to empirically assert the continuity of the solution.